#include <limits>
#include <math.h>
#include "FakeSpring.h"

void FakeSpring::updateForce(Particle *particle, float frameTime)
{
	// Check that we do not have infiniate mass.
	if (particle->mass == std::numeric_limits<float>::infinity())
		return;

	// Calculate the relative position of the particle to the anchor.
	D3DXVECTOR3 position = particle->position;
	position -= *anchor;

	// Calculate the constants and check that they are in bounds.
	float gamma = 0.5f * sqrt(4 * springConstant - damping * damping);
	if (gamma == 0.0f) return;
	D3DXVECTOR3 c = position * (damping / (2.0f * gamma)) +
		particle->velocity * (1.0f / gamma);

	// Calculate the target position.
	D3DXVECTOR3 target = position * cos(gamma * frameTime) + 
		c * sin(gamma * frameTime);
	target *= exp(-0.5f * frameTime * damping);

	// Calculate the resulting acceleration, and therefore the force.
	D3DXVECTOR3 accel = (target - position) * (1.0f / frameTime * frameTime) - 
		particle->velocity * frameTime;
	particle->force += (accel * particle->mass);
}